#ifndef _PID_H_
#define _PID_H_

#include "reg52.h"
#include "pwm.h"
#include "loss.h"
typedef unsigned int u16;

extern u8 basicV; //基准速度，m基于此值加减
//此处PID仅处理转弯时考虑曲率的速度更新（计算m），平道加速不在考虑范围内
static int lastLoss;
static int last2Loss;
static int nowloss;

//fix:参数还没调
#define k1 1
#define k2 1
#define k3 1

void setNowLoss(int newloss)
{
	last2Loss=lastLoss;
	lastLoss=nowloss;
	nowloss=newloss;
}

/*偏左误差为正，偏右误差为负，因为S形赛道的存在，两个时刻误差正负相反是有可能出现的，因此差分值可能为正
也可能为负*/

int der1()
{
	return nowloss-lastLoss;
}

int der2()
{
	return nowloss+last2Loss-2*lastLoss;
}

int getm(int newloss)
{
	setNowLoss(newloss);
	return k1*newloss+k2*der1()+k3*der2();
	//注意，此式子使用泰勒公式得到，首先，将误差转换为m需要一个系数k，这很容易理解
	//因为考虑后效性，所以需要考虑下一时刻的误差进行计算，即改为计算两个数的加权平均：(k1*loss+k2*loss2)/2
	//其中，loss2使用泰勒公式得到，泰勒公式中，一阶差分和二阶差分均有原始系数（设为c2 c3）
	//直接展开结果为(k1+1)loss+k2*c2*der1+k2*c3*der2。因为二阶逼近的误差，所以二阶和三阶差分的权重还需要进一步进行调整
	//即将式子改为k1*loss+k2*der1+k3*der2，其中，k2和k3依赖误差到m的量度转换系数（原来的k）和赋予二阶，三阶差分的权重
}

u8 normali(int val)
{
	if(val>20)
		return 20;
	if(val<0)
		return 0;
	return val;
}

void resetV() //接口函数，当确认有偏移，修正误差时使用
{
	int m=getm(getLoss());
	u8 v1=normali(basicV+m);
	u8 v2=normali(basicV-m);
	setV(v1,v2); //对于m，一直是左边加右边减，从这点也可以看出m（各阶差分值是加权和）一定有正负
}

void recoverV() //接口函数，在线后恢复原先速度
{
	setV(basicV,basicV);
}

void setBasicV(u8 v) //接口函数，因为pid要记录平道速度，所以平时就用这个设置速度即可，不要用setV
{
	basicV=v;
	setV(v,v);
}

#endif